use cortex_m::asm::delay;
use stm32f4::stm32f407::{FLASH, PWR, RCC};

#[cfg(feature = "pllm_8")]
const PLLM: u8 = 8;

#[cfg(feature = "pllm_25")]
const PLLM: u8 = 25;

const PLLN: u16 = 336;

const PLLQ: u8 = 7;

pub unsafe fn set_rcc_to168() {
    let rcc = unsafe { RCC::steal() };

    /* Reset the RCC clock configuration to the default reset state ------------*/
    /* Set HSION bit */
    rcc.cr().modify(|r, w| unsafe { w.bits(r.bits() | 0x01) });

    /* Reset CFGR register */
    rcc.cfgr().write(|w| unsafe { w.bits(0) });

    /* Reset HSEON, CSSON and PLLON bits */
    rcc.cr()
        .modify(|r, w| unsafe { w.bits(r.bits() & 0xFEF6FFFF) });

    /* Reset PLLCFGR register */
    rcc.pllcfgr().write(|w| unsafe { w.bits(0x24003010) });

    /* Reset HSEBYP bit */
    rcc.cr()
        .modify(|r, w| unsafe { w.bits(r.bits() & 0xFFFBFFFF) });

    /* Disable all interrupts */
    rcc.cir().write(|w| unsafe { w.bits(0) });

    let mut time_count = 0;
    const HSE_STARTUP_TIMEOUT: u32 = 0x05000;

    /* Enable HSE */
    rcc.cr().modify(|_, w| w.hseon().on());

    while rcc.cr().read().hserdy().is_not_ready() {
        time_count += 1;
        delay(1);
        if time_count >= HSE_STARTUP_TIMEOUT {
            #[cfg(feature = "defmt")]
            defmt::error!("rcc hse on timeout");
            return;
        }
    }

    rcc.apb1enr().modify(|_, w| w.pwren().enabled());

    let pwr = unsafe { PWR::steal() };
    pwr.cr().modify(|_, w| w.vos().scale1());

    rcc.cfgr()
        .modify(|_, w| w.hpre().div1().ppre2().div2().ppre1().div4());

    rcc.pllcfgr().write(|w| unsafe {
        w.pllm()
            .bits(PLLM)
            .plln()
            .bits(PLLN)
            .pllp()
            .div2()
            .pllsrc()
            .hse()
            .pllq()
            .bits(PLLQ)
    });

    rcc.cr().modify(|_, w| w.pllon().on());
    while rcc.cr().read().pllrdy().is_not_ready() {}

    let flash = unsafe { FLASH::steal() };
    flash.acr().write(|w| {
        w.prften()
            .enabled()
            .icen()
            .enabled()
            .dcen()
            .enabled()
            .latency()
            .ws5()
    });

    rcc.cfgr().modify(|_, w| w.sw().pll());
    while !rcc.cfgr().read().sws().is_pll() {}
}






